/*
  Phidget sensor class, encapsulates IMU and Encoder drivers
  Author: SUMo
  Version: March 26
*/

#ifndef DUAL_NODE
#define DUAL_NODE

#include <unistd.h>	// for the sleep(second) function
#include <stdio.h>	// for fscanf etc.
// ROS related functions: nodehandle, publisher etc.
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>
// Phidget
#include <phidget21.h>
// Include config constants;
#include "config.h"

// Sensor fusion algorithm
#ifdef USE_MAHONY
#include "MahonyAHRS.h"
#else
#include "MadgwickAHRS.h"
#endif

#define CALIBRATING true

class PhidgetSensorNode
{
 public:
  CPhidgetSpatialHandle imu_handle;
  CPhidgetEncoderHandle enc_handle;

  // ROS node handlers
  ros::NodeHandle nh;
  ros::Publisher imu_pub;
  ros::Publisher odom_pub;

  sensor_msgs::Imu imu_pose;
  nav_msgs::Odometry enc_odom;

  // IMU specific data
#ifdef USE_MAHONY
  MahonyAHRS ahrs;
#else
  Madgwick ahrs;
#endif

  // Encoder specific data
  
  // public:
  PhidgetSensorNode(ros::NodeHandle n, int sample_period_msec);

  ~PhidgetSensorNode();

  //Both functions are called inside IMU event handler
  void processImuData(CPhidgetSpatial_SpatialEventDataHandle sd);
  void processEncoderData();

 private:
  
  void init_phidget_imu(void);
  void init_phidget_enc(void);

  void init_ros_imu(void);
  void init_ros_enc(void);

  void config_imu(int sample_period_msec);
  void config_enc(void);

  void setEncoderCovariance(double value);
};
#endif
